import numpy  as np
from math import sqrt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt


def rigid_transform_3D(A, B):
    assert len(A) == len(B)
    N = A.shape[0]

    mu_A = np.mean(A, axis=0)
    mu_B = np.mean(B, axis=0)
    AA = A - np.tile(mu_A, (N, 1))
    BB = B - np.tile(mu_B, (N, 1))

    H = np.transpose(AA) * BB

    print("---------")
    print(A)
    print(mu_A)
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T * U.T

    if np.linalg.det(R) < 0:
        print("Reflection detected")
        Vt[2, :] *= -1
        R = Vt.T * U.T

    t = -R * mu_A.T + mu_B.T
    print("mu_B.T", mu_B.T)
    return R, t


if __name__ == '__main__':
    sumX = 0
    sumY = 0
    sumZ = 0
    qiusumX = 0
    qiusumY = 0
    qiusumZ = 0

    ping = np.mat([[-115.305, -650.432, 386.146, 179.662, -18.297, -173.293],
                   [-70.965, -650.417, 386.12, 179.658, -18.295, -173.291],
                   [-79.498, -677.084, 389.977, 179.657, -18.292, -173.286],
                   [-97.341, - 679.782, 397.242, 179.658, -18.282, -173.276]])
    pingA = np.mat([[-115.305, -650.432, 386.146],
                    [-70.965, -650.417, 386.12],
                    [-79.498, -677.084, 389.977],
                    ])
    ceshiA = np.mat([-97.341, - 679.782, 397.242])
    xuan = np.mat([[-97.289, -680.188, 402.53, - 179.551, - 14.948, -173.269],
                   [-97.245, -680.188, 402.514, - 179.852, - 11.719, -179.904],
                   [-97.209, -680.196, 402.493, - 179.957, - 16.28, -178.086],
                   [-97.195, -680.197, 402.474, - 179.957, - 18.586, -178.312]])

    qiuxin = np.mat([[-35.836487, 12.679420, 24.244864],
                     [6.236456, 8.846513, 10.642600],
                     [-5.229183, -17.158829, 10.737526],
                     ])
    ceshiB = np.mat([-24.381903, -18.477112, 9.731645])
    qiuxin2 = np.mat([[-2.906959, -23.567701, 14.165867],
                      [16.804220, 10.097496, 24.152893],
                      [-14.867281, 18.608458, 10.132659],
                      [-28.577002, 1.602351, 3.618488]])
    # ---------------------------------------vs 计算的旋转矩阵 xuan-------------------------
    xuanz1 = np.mat([[-0.9595009484360865, -0.1192118970908492, -0.2552380331029281],
                     [-0.1132417938149584, 0.9928399667288341, -0.03801442620187726],
                     [0.2579422921581901, -0.007571265276577524, -0.9661306587922209]])
    xuanz2 = np.mat([[-0.9791541355504213, -0.002200164432022653, -0.2031067160656666],
                     [-0.001640590036027432, 0.9999943810915645, -0.002923396933802634],
                     [0.2031120067815808, -0.002529241342960684, -0.9791522433408342]])
    xuanz3 = np.mat([[-0.9593676600422083, -0.03360964889793703, -0.2801501104159969],
                     [-0.03206018068269702, 0.9994347765241837, -0.01011297625073053],
                     [0.2803316565779375, -0.0007203992035113897, -0.9599029343351633]])
    xuanz4 = np.mat([[-0.9474350016705273, -0.02969598645566099, -0.3185672079765792],
                     [-0.0279206105213066, 0.9995587238220328, -0.01013889241418117],
                     [0.3187277162684246, -0.0007113506125928877, -0.9478460512459924]])
    Txuan1 = np.mat([[-97.289],
                     [-680.188],
                     [402.53]])
    Txuan2 = np.mat([[-97.245],
                     [-680.188],
                     [402.514]])
    Txuan3 = np.mat([[-97.209],
                     [-680.196],
                     [402.493]])
    Txuan4 = np.mat([[-97.19499999999999],
                     [-680.197],
                     [402.474]])
    # -----------------------------------------
    ret_R, ret_t = rigid_transform_3D(pingA, qiuxin)
    print("--------------------------------")
    print(ret_R, "\n", ret_t)
    B1 = ret_R * ceshiA.T + ret_t
    print("B1\n", B1)

    R1R2T = xuanz2 * ret_R * qiuxin2[1].T - xuanz1 * ret_R * qiuxin2[0].T + Txuan2 - Txuan1
    R1R2 = xuanz1 - xuanz2
    R1R2_inv = np.linalg.inv(R1R2)
    T21 = R1R2_inv * R1R2T
    D23 = xuanz3 * ret_R * qiuxin2[2].T - xuanz2 * ret_R * qiuxin2[1].T + Txuan3 - Txuan2
    C23 = xuanz2 - xuanz3
    Ts23 = np.linalg.inv(C23.T * C23) * C23.T *D23

    # T32 = R2R3_inv*R2R3T

    print(R1R2,"\n", R1R2_inv,"\n", T21,"ttt\n",Ts23)
    # R = np.mat(np.random.rand(3, 3))
    # t = np.mat(np.random.rand(3, 1))
    #
    # U, S, Vt = np.linalg.svd(R)
    # R = U * Vt
    # if np.linalg.det(R) < 0:
    #     Vt[2, :] *= -1
    #     R = U * Vt
    #
    # n = 10
    # A = np.mat(np.random.rand(n, 3))
    # B = R * A.T + np.tile(t, (1, n))
    # B = B.T
    #
    # ret_R, ret_t = rigid_transform_3D(A, B)
    # A2 = (ret_R * A.T) + np.tile(ret_t, (1, n))
    # A2 = A2.T
    #
    # err = A2 - B
    #
    # err = np.multiply(err, err)
    # err = sum(err)
    # # rmse = sqrt(err / n)
    #
    # print("points A2")
    # print(A2)
    # print("")
    #
    # print("points B")
    # print(B)
    # print("")
    #
    # print(err)
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')
    # ax.scatter(A[:, 0], A[:, 1], A[:, 2])
    # ax.scatter(B[:, 0], B[:, 1], B[:, 2], s=100, marker='x')
    # ax.scatter(A2[:, 0], A2[:, 1], A2[:, 2], s=100, marker='o')
    # plt.legend()
    # plt.show()

# https://blog.csdn.net/u012836279/article/details/80351462?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-0&spm=1001.2101.3001.4242
